
#include "mb_datamodel.h"
#include "mb_adu.h"
#include "mb_debug.h"
#include "mb_functioncode.h"
#include "mb_holdingregisters.h"
#include "mb_comm.h"
#include "mb_crc.h"

/**
 * @brief 03 读保持寄存器 响应
 * @param hslave        从站指针
 * @param pReqbuf       指针,需要解析的数据指针
 * @param pRspbuf       指针,响应数据缓冲区
 * @param pRspbuflen    指针,响应数据长度
 * @ret   MB_OK 成功 其它 出错
*/
MB_StatusTypeDef MB_HOLDREG_Read_Response(
    MB_SLAVE_HandleTypeDef *hslave, 
    uint8_t *pReqbuf, 
    uint8_t *pRspbuf, 
    uint16_t *pRspbuflen)
{
    uint16_t rspDataLen = 0;
    uint16_t rspBufLen = 0;
    uint16_t crc    = 0;

    MB_READ_REQADU_HandleTypeDef *pReqADU = (MB_READ_REQADU_HandleTypeDef *)pReqbuf;  
    MB_READ_RSPADU_HandleTypeDef *pRspADU = (MB_READ_RSPADU_HandleTypeDef *)pRspbuf;  
    //判断线圈数量是不是 < 2000 (GB/T 1958.2.1 2008  第十三页)
    if(MB_PP_NTOHS(pReqADU->Pdu.Quantity) < 0x7d0 && pReqADU->Pdu.Quantity >= 1) //2000
    {
        //判断开始地址加上长度是不是在定义的寄存器范围内
        if(MB_PP_NTOHS(pReqADU->Pdu.StartAddress) + MB_PP_NTOHS(pReqADU->Pdu.Quantity) < hslave->Regedit.HoldingRegistersBufSize)
        {
            //处理请求
            pRspADU->AdditionalAddress = hslave->SlaveID;
            pRspADU->Pdu.FunctionCode  = MB_FUN_READ_HOLDING_REGISTERS;
            //从内存中读取输入
            if(MB_ByteArrayCopyFrom(
                 hslave->Regedit.pHoldingRegistersBuffPtr,
                 MB_PP_NTOHS(pReqADU->Pdu.StartAddress),
                 MB_PP_NTOHS(pReqADU->Pdu.Quantity),
						     pRspADU->Pdu.Data,                 
                 &rspDataLen
                 ) == MB_OK)
                 {
                    //计算长度
                    pRspADU->Pdu.ByteCount = rspDataLen;
                    rspBufLen = rspDataLen + 3;
                    crc = MB_GenerateCRC16CheckCode(pRspbuf, rspBufLen);
                    pRspADU->Pdu.Data[rspDataLen] = (crc & 0xff00) >> 8;
                    pRspADU->Pdu.Data[rspDataLen + 1] = (crc & 0x00ff);
                    *pRspbuflen = rspBufLen + 2;
									   return MB_OK;
                 } else {
                    return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 4, pReqbuf, pRspbuflen); 
                 }
        } else {
             return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 2, pReqbuf, pRspbuflen);
        }

    } else {
        return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 3, pReqbuf, pRspbuflen);
    }
}

/**
 * @brief 06 写单个寄存器功能码 响应
 * @param hslave        从站指针
 * @param pReqbuf       指针,需要解析的数据指针
 * @param pRspbuf       指针,响应数据缓冲区
 * @param pRspbuflen    指针,响应数据长度
 * @ret   MB_OK 成功 其它 出错
*/
MB_StatusTypeDef MB_HOLDREG_WriteSingle_Response(
    MB_SLAVE_HandleTypeDef *hslave,
    uint8_t *pReqbuf,
    uint8_t *pRspbuf,
    uint16_t *pRspbuflen)
{
    uint16_t rspDataLen = 0;
    uint16_t crc = 0;

    MB_WRITE_SINGLE_REQADU_HandleTypeDef *pReqADU = (MB_WRITE_SINGLE_REQADU_HandleTypeDef *)pReqbuf;
    MB_WRITE_SINGLE_RSPADU_HandleTypeDef *pRspADU = (MB_WRITE_SINGLE_RSPADU_HandleTypeDef *)pRspbuf;
        //判断开始地址加上长度是不是在定义的寄存器范围内
        if (MB_PP_NTOHS(pReqADU->Pdu.StartAddress) < hslave->Regedit.HoldingRegistersBufSize)
        {
            //处理请求
            pRspADU->AdditionalAddress = hslave->SlaveID;
            pRspADU->Pdu.FunctionCode = MB_FUN_WRITE_SINGLE_REGISTER;
            //从内存中读取线圈
            if (MB_ByteArrayCopyTo(
                    (uint8_t *)&pReqADU->Pdu.Value,
                    MB_PP_NTOHS(pReqADU->Pdu.StartAddress),
                    1,
                    hslave->Regedit.pHoldingRegistersBuffPtr,
                    &rspDataLen) == MB_OK)
            {
                pRspADU->Pdu.StartAddress = pReqADU->Pdu.StartAddress;
                pRspADU->Pdu.Value = pReqADU->Pdu.Value;
                rspDataLen = 6;
                crc = MB_GenerateCRC16CheckCode(pRspbuf, rspDataLen);
                pRspADU->CRC = MB_PP_HTONS(crc);
                *pRspbuflen = rspDataLen + 2;
							return MB_OK;
            }
            else
            {
                return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 4, pRspbuf, pRspbuflen);
            }
        }
        else
        {
            return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 2, pRspbuf, pRspbuflen);
        }
}

/**
 * @brief 16(0x10) 写多个寄存器功能码 响应
 * @param hslave        从站指针
 * @param pReqbuf       指针,需要解析的数据指针
 * @param pRspbuf       指针,响应数据缓冲区
 * @param pRspbuflen    指针,响应数据长度
 * @ret   MB_OK 成功 其它 出错
*/
MB_StatusTypeDef MB_HOLDREG_WriteMultiple_Response(
    MB_SLAVE_HandleTypeDef *hslave,
    uint8_t *pReqbuf,
    uint8_t *pRspbuf,
    uint16_t *pRspbuflen)
{
    uint16_t rspDataLen = 0;
    uint16_t crc = 0;

    MB_WRITE_MULTIPLE_REQADU_HandleTypeDef *pReqADU = (MB_WRITE_MULTIPLE_REQADU_HandleTypeDef *)pReqbuf;
    MB_WRITE_MULTIPLE_RSPADU_HandleTypeDef *pRspADU = (MB_WRITE_MULTIPLE_RSPADU_HandleTypeDef *)pRspbuf;
    //判断寄存器数量是不是 1 <<  Quantity << 123
    if (MB_PP_NTOHS(pReqADU->Pdu.Quantity) >= 0x0001 && MB_PP_NTOHS(pReqADU->Pdu.Quantity) <= 0x07B && pReqADU->Pdu.ByteCount == (MB_PP_NTOHS(pReqADU->Pdu.Quantity) << 1))
    {
        //判断开始地址加上长度是不是在定义的寄存器范围内
        if (MB_PP_NTOHS(pReqADU->Pdu.StartAddress) + (MB_PP_NTOHS(pReqADU->Pdu.Quantity) / 8 + 1) < hslave->Regedit.HoldingRegistersBufSize)
        {
            //处理请求
            pRspADU->AdditionalAddress = hslave->SlaveID;
            pRspADU->Pdu.FunctionCode = MB_FUN_WRITE_MULTIPLE_REGISTERS;
            //保存数据
            if (MB_ByteArrayCopyTo(
                    (uint8_t *)&pReqADU->Pdu.Data,
                    MB_PP_NTOHS(pReqADU->Pdu.StartAddress),
                    MB_PP_NTOHS(pReqADU->Pdu.Quantity),
                    hslave->Regedit.pHoldingRegistersBuffPtr,
                    &rspDataLen) == MB_OK)
            {
                pRspADU->Pdu.StartAddress = pReqADU->Pdu.StartAddress;
                pRspADU->Pdu.Quantity     = pReqADU->Pdu.Quantity;
                rspDataLen = 6;
                crc = MB_GenerateCRC16CheckCode(pRspbuf, rspDataLen);
                pRspADU->CRC = MB_PP_HTONS(crc);
                *pRspbuflen = rspDataLen + 2;
							  return MB_OK;
            }
            else
            {
                return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 4, pRspbuf, pRspbuflen);
            }
        }
        else
        {
            return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 2, pRspbuf, pRspbuflen);
        }
    }
    else
    {
        return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 3, pRspbuf, pRspbuflen);
    }
}

/**
 * @brief 23(0x17)      读写多个寄存器功能码 响应
 * @param hslave        从站指针
 * @param pSrc          需要解析的数据指针
 * @ret   MB_OK 成功 其它 出错
*/
MB_StatusTypeDef MB_HOLDREG_ReadWriteMultiple_Response(
    MB_SLAVE_HandleTypeDef *hslave,
    uint8_t *pReqbuf,
    uint8_t *pRspbuf,
    uint16_t *pRspbuflen)
{
    uint16_t rspDataLen = 0;
    uint16_t rspBufLen = 0;
    uint16_t crc = 0;

    MB_READWRITE_MULTIPLE_REQADU_HandleTypeDef *pReqADU = (MB_READWRITE_MULTIPLE_REQADU_HandleTypeDef *)pReqbuf;
    MB_READ_RSPADU_HandleTypeDef *pRspADU = (MB_READ_RSPADU_HandleTypeDef *)pRspbuf;
    //判断寄存器数量是不是 1 <<  Quantity << 123
    if (MB_PP_NTOHS(pReqADU->Pdu.WriteQuantity) >= 0x0001 && MB_PP_NTOHS(pReqADU->Pdu.WriteQuantity) <= 0x07B && pReqADU->Pdu.WriteByteCount == (MB_PP_NTOHS(pReqADU->Pdu.WriteQuantity) << 1))
    {
        //判断开始地址加上长度是不是在定义的寄存器范围内
        if (MB_PP_NTOHS(pReqADU->Pdu.WriteStartAddress) + (MB_PP_NTOHS(pReqADU->Pdu.WriteQuantity) * 2) < hslave->Regedit.HoldingRegistersBufSize)
        {
            //处理请求
            pRspADU->AdditionalAddress = hslave->SlaveID;
            pRspADU->Pdu.FunctionCode = MB_FUN_READ_WRITE_MULTIPLE_REGISTERS;
            //保存数据
            if (MB_ByteArrayCopyTo(
                    (uint8_t *)&pReqADU->Pdu.Data,
                    MB_PP_NTOHS(pReqADU->Pdu.WriteStartAddress),
                    MB_PP_NTOHS(pReqADU->Pdu.WriteQuantity),
                    hslave->Regedit.pHoldingRegistersBuffPtr,
                    &rspDataLen) == MB_OK)
            {
               if(MB_ByteArrayCopyFrom(
                 hslave->Regedit.pHoldingRegistersBuffPtr,
                 MB_PP_NTOHS(pReqADU->Pdu.ReadStartAddress),
                 MB_PP_NTOHS(pReqADU->Pdu.ReadQuantity),
							   pRspADU->Pdu.Data,
                 &rspDataLen
                 ) == MB_OK)
                 {
                    //计算长度
                    pRspADU->Pdu.ByteCount = rspDataLen;
                    rspBufLen = rspDataLen + 3;
                    crc = MB_GenerateCRC16CheckCode(pRspbuf, rspBufLen);
                    pRspADU->Pdu.Data[rspDataLen] = (crc & 0xff00) >> 8;
                    pRspADU->Pdu.Data[rspDataLen + 1] = (crc & 0x00ff);
                   *pRspbuflen = rspBufLen + 2;
									  return MB_OK;
                 } else {
                    return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 4, pRspbuf, pRspbuflen); 
                 }
            }
            else
            {
                return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 4, pRspbuf, pRspbuflen);
            }
        }
        else
        {
            return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 2, pRspbuf, pRspbuflen);
        }
    }
    else
    {
        return MB_SLAVE_ExceptionRsp(hslave, pReqADU->Pdu.FunctionCode, 3, pRspbuf, pRspbuflen);
    }
}
